#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/WrenchStamped.h>
#include <sensor_msgs/Illuminance.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Range.h>
#include <signal.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/UInt8MultiArray.h>
#include <webots_ros/BoolStamped.h>
#include <webots_ros/Float64Stamped.h>
#include <webots_ros/Int32Stamped.h>
#include <webots_ros/Int8Stamped.h>
#include <webots_ros/RadarTarget.h>
#include <webots_ros/RecognitionObject.h>
#include <webots_ros/RecognitionObjects.h>
#include <webots_ros/StringStamped.h>
#include <cstdlib>
#include "ros/ros.h"

#include <webots_ros/get_bool.h>
#include <webots_ros/get_float.h>
#include <webots_ros/get_float_array.h>
#include <webots_ros/get_int.h>
#include <webots_ros/get_string.h>
#include <webots_ros/get_uint64.h>
#include <webots_ros/get_urdf.h>
#include <webots_ros/set_bool.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_float_array.h>
#include <webots_ros/set_int.h>
#include <webots_ros/set_string.h>

#include <webots_ros/camera_get_focus_info.h>
#include <webots_ros/camera_get_info.h>
#include <webots_ros/camera_get_zoom_info.h>
#include <webots_ros/display_draw_line.h>
#include <webots_ros/display_draw_oval.h>
#include <webots_ros/display_draw_pixel.h>
#include <webots_ros/display_draw_polygon.h>
#include <webots_ros/display_draw_rectangle.h>
#include <webots_ros/display_draw_text.h>
#include <webots_ros/display_get_info.h>
#include <webots_ros/display_image_copy.h>
#include <webots_ros/display_image_delete.h>
#include <webots_ros/display_image_load.h>
#include <webots_ros/display_image_new.h>
#include <webots_ros/display_image_paste.h>
#include <webots_ros/display_image_save.h>
#include <webots_ros/display_set_font.h>
#include <webots_ros/field_disable_sf_tracking.h>
#include <webots_ros/field_enable_sf_tracking.h>
#include <webots_ros/field_get_bool.h>
#include <webots_ros/field_get_color.h>
#include <webots_ros/field_get_count.h>
#include <webots_ros/field_get_float.h>
#include <webots_ros/field_get_int32.h>
#include <webots_ros/field_get_name.h>
#include <webots_ros/field_get_node.h>
#include <webots_ros/field_get_rotation.h>
#include <webots_ros/field_get_string.h>
#include <webots_ros/field_get_type.h>
#include <webots_ros/field_get_vec2f.h>
#include <webots_ros/field_get_vec3f.h>
#include <webots_ros/field_import_node.h>
#include <webots_ros/field_remove.h>
#include <webots_ros/field_set_bool.h>
#include <webots_ros/field_set_color.h>
#include <webots_ros/field_set_float.h>
#include <webots_ros/field_set_int32.h>
#include <webots_ros/field_set_rotation.h>
#include <webots_ros/field_set_string.h>
#include <webots_ros/field_set_vec2f.h>
#include <webots_ros/field_set_vec3f.h>
#include <webots_ros/lidar_get_frequency_info.h>
#include <webots_ros/lidar_get_info.h>
#include <webots_ros/motor_set_control_pid.h>
#include <webots_ros/node_add_force_or_torque.h>
#include <webots_ros/node_add_force_with_offset.h>
#include <webots_ros/node_disable_contact_points_tracking.h>
#include <webots_ros/node_disable_pose_tracking.h>
#include <webots_ros/node_enable_contact_points_tracking.h>
#include <webots_ros/node_enable_pose_tracking.h>
#include <webots_ros/node_get_center_of_mass.h>
#include <webots_ros/node_get_contact_points.h>
#include <webots_ros/node_get_field.h>
#include <webots_ros/node_get_field_by_index.h>
#include <webots_ros/node_get_id.h>
#include <webots_ros/node_get_name.h>
#include <webots_ros/node_get_number_of_fields.h>
#include <webots_ros/node_get_orientation.h>
#include <webots_ros/node_get_parent_node.h>
#include <webots_ros/node_get_pose.h>
#include <webots_ros/node_get_position.h>
#include <webots_ros/node_get_static_balance.h>
#include <webots_ros/node_get_status.h>
#include <webots_ros/node_get_string.h>
#include <webots_ros/node_get_type.h>
#include <webots_ros/node_get_velocity.h>
#include <webots_ros/node_remove.h>
#include <webots_ros/node_reset_functions.h>
#include <webots_ros/node_set_joint_position.h>
#include <webots_ros/node_set_string.h>
#include <webots_ros/node_set_velocity.h>
#include <webots_ros/node_set_visibility.h>
#include <webots_ros/pen_set_ink_color.h>
#include <webots_ros/range_finder_get_info.h>
#include <webots_ros/receiver_get_emitter_direction.h>
#include <webots_ros/robot_get_device_list.h>
#include <webots_ros/robot_set_mode.h>
#include <webots_ros/robot_wait_for_user_input_event.h>
#include <webots_ros/save_image.h>
#include <webots_ros/skin_get_bone_name.h>
#include <webots_ros/skin_get_bone_orientation.h>
#include <webots_ros/skin_get_bone_position.h>
#include <webots_ros/skin_set_bone_orientation.h>
#include <webots_ros/skin_set_bone_position.h>
#include <webots_ros/speaker_play_sound.h>
#include <webots_ros/speaker_speak.h>
#include <webots_ros/supervisor_get_from_def.h>
#include <webots_ros/supervisor_get_from_id.h>
#include <webots_ros/supervisor_get_from_string.h>
#include <webots_ros/supervisor_movie_start_recording.h>
#include <webots_ros/supervisor_set_label.h>
#include <webots_ros/supervisor_virtual_reality_headset_get_orientation.h>
#include <webots_ros/supervisor_virtual_reality_headset_get_position.h>
#include <math.h>
#include <chrono>
using namespace std;

static bool callbackCalled = false;

string model_name = "my_robot";

static int model_count;
static vector<string> model_list;
ros::ServiceClient time_step_client;
webots_ros::set_int time_step_srv;
double x,y,theta=M_PI/2;
// ,dis;
// double position1=M_PI/2,position2;
double l=0.18,d=0.08,r=0.025;

double P1=100,P1_,t1,t1_,P2=100,P2_,t2,t2_;

void modelNameCallback(const std_msgs::String::ConstPtr &name) {
  model_count++;
  model_list.push_back(name->data);
  ROS_INFO("Model #%d: %s.", model_count, model_list.back().c_str());
  callbackCalled = true;
}

void pre_treatment(ros::NodeHandle &n)
{
  ros::Subscriber name_sub = n.subscribe("model_name", 100, modelNameCallback);
  while (model_count == 0 || model_count < name_sub.getNumPublishers()) {
    ros::spinOnce();
    ros::spinOnce();
    ros::spinOnce();
  }
  ros::spinOnce();
  name_sub.shutdown();

  //begin main loop

  string model;
  string path;
  string data;
  vector<string> device_list;

  //get_model

  if (time_step_client.call(time_step_srv) && time_step_srv.response.success)
    ROS_INFO("time_step service works.");
  else
    ROS_ERROR("Failed to call service time_step to update robot's time step.");

  //get_number_of_devices_client
  ros::ServiceClient get_number_of_devices_client =
    n.serviceClient<webots_ros::get_int>(model_name + "/robot/get_number_of_devices");
  webots_ros::get_int get_number_of_devices_srv;

  if (get_number_of_devices_client.call(get_number_of_devices_srv)) {
    int number_of_devices = get_number_of_devices_srv.response.value;
    ROS_INFO("%s has %d devices.", model_name.c_str(), number_of_devices);
  } else
    ROS_ERROR("Failed to call service get_number_of_devices.");

  get_number_of_devices_client.shutdown();
  time_step_client.call(time_step_srv);

  //get_device_list
  ros::ServiceClient device_list_client =
    n.serviceClient<webots_ros::robot_get_device_list>(model_name + "/robot/get_device_list");
  webots_ros::robot_get_device_list device_list_srv;

  if (device_list_client.call(device_list_srv)) {
    device_list = device_list_srv.response.list;
    for (unsigned int i = 0; i < device_list.size(); i++)
      ROS_INFO("Device [%d]: %s.", i, device_list[i].c_str());
  } else
    ROS_ERROR("Failed to call service device_list.");

  device_list_client.shutdown();
  time_step_client.call(time_step_srv);
}

void callback1(const webots_ros::Float64Stamped::ConstPtr & value)
{
  P1=value->data;
  t1=double(value->header.stamp.sec)+double(value->header.stamp.nsec)/(1e9);
  // ROS_INFO("pos1: %f, t: %f",P1,t1);
  // callbackCalled=true;
}

void callback2(const webots_ros::Float64Stamped::ConstPtr & value)
{
  P2=value->data;
  t2=double(value->header.stamp.sec)+double(value->header.stamp.nsec)/(1e9);
  // ROS_INFO("pos1: %f, t: %f",P2,t2);
  // ROS_INFO("pos2: %f",P2);
  // callbackCalled=true;
}

int main(int argc,char **argv)
{
    ros::init(argc,argv,"odom");
    ros::NodeHandle n;

    ros::Publisher odom;

    ros::ServiceClient set_pos1,set_pos2;
    ros::Subscriber pos1,pos2;
    webots_ros::set_int set_pos_srv1,set_pos_srv2;

    double delta_dis=0,theta_temp=0,rou_temp=0;

    pre_treatment(n);
    std_msgs::Float64MultiArray msg;
    msg.data.push_back(0);
    msg.data.push_back(l/2);
    msg.data.push_back(M_PI/2);

    odom = n.advertise<std_msgs::Float64MultiArray>(model_name+"/odom", 10);
    //pre_treat
    set_pos1=n.serviceClient<webots_ros::set_int>(model_name + "/position3/enable");
    set_pos2=n.serviceClient<webots_ros::set_int>(model_name + "/position4/enable");
    set_pos_srv1.request.value=5;
    set_pos_srv2.request.value=5;
    set_pos1.call(set_pos_srv1);
    set_pos2.call(set_pos_srv2);
    
    ROS_INFO("%d , %d",set_pos_srv1.response.success,set_pos_srv2.response.success);

    pos1=n.subscribe<webots_ros::Float64Stamped>(model_name + "/position3/value",10,callback1);
    pos2=n.subscribe<webots_ros::Float64Stamped>(model_name + "/position4/value",10,callback2);

    ros::Rate loop_rate(200);

    while(true)
    {
      
      ros::spinOnce();

      if (P1==100||P2==100)
        continue;
      // auto start = std::chrono::steady_clock::now();

      odom.publish(msg);

      // position1=fmod(position1+2*M_PI,2*M_PI);
      rou_temp=sin((P1+P1_)/2)/l;
      delta_dis=(P2-P2_)*r;
      theta=theta-delta_dis*rou_temp;

      double check=fmod(((P1+P1_)/2+1000000*2*M_PI),2*M_PI);
      // theta=fmod(theta+2*M_PI,2*M_PI);
  
      double rate_temp;
      // if (fabs(rou_temp)<1e-5)
      // rate_temp=1;
      // else
      rate_temp=sqrt(1-l*l*(rou_temp*rou_temp));


      if ((check>=0&&check<M_PI/2)||(check>=3*M_PI/2&&check<2*M_PI))
      {
        x+=cos(theta)*delta_dis*rate_temp;
        y+=sin(theta)*delta_dis*rate_temp;
      }
      if ((check>=M_PI/2&&check<M_PI)||(check>=M_PI&&check<3*M_PI/2))
      {
        x-=cos(theta)*delta_dis*rate_temp;
        y-=sin(theta)*delta_dis*rate_temp;
      }
      
      // if(check>=M_PI&&check<3*M_PI/2)
      // {
      //   x+=cos(theta)*delta_dis*rate_temp;
      //   y-=sin(theta)*delta_dis*rate_temp;
      // }
      // if(check>=3*M_PI/2&&check<2*M_PI)
      // {
      //   x-=cos(theta)*delta_dis*rate_temp;
      //   y+=sin(theta)*delta_dis*rate_temp;
      // }

      msg.data[0]=x;
      msg.data[1]=y;
      msg.data[2]=theta;
      
      P1_=P1;
      P2_=P2;

      ROS_INFO("%f  %f  %f",x,y,theta);
      odom.publish(msg);

      loop_rate.sleep();
      // auto finish = std::chrono::steady_clock::now();
      // auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(finish - start);
      // std::cout << duration.count() << "ms" << std::endl;
  }
}
